#include <iostream>
#include <cstdlib>
#include <fstream>
#include <string>
#include <cstring>
#include "timetools.h"
#include <errno.h>

#include "arglib.h"


using namespace std;


std::vector<double*> matricesA;
std::vector<double*> matricesB;
std::vector<double*> matricesC;


int next_matrix = 0;

clarg::argInt nmats("-nm", "Number of matrix multiplications", 1);
clarg::argInt matsz("-msz", "Matrix size = N x N", 1000);
clarg::argInt nthreads("-nt", "Number of threads", 1);


double* new_matrix(double* src, unsigned N) {
  
  double *m = (double*) malloc( N*N*sizeof(double) );
  if (m == NULL) {
	printf("\n\nError: new_matrix()\n\n");
	exit( -999 );
  }

  if (src != NULL) { memcpy( m, src, N*N*sizeof(double) ); }

  else { memset( m, 0, N*N*sizeof(double) ); }
	
  return m;
}


void init_matrices(void) {
	
  for (unsigned i=0; i<nmats.get_value(); i++) {
    matricesA.push_back( new_matrix(0, matsz.get_value()) );
    matricesB.push_back( new_matrix(0, matsz.get_value()) );
    matricesC.push_back( new_matrix(0, matsz.get_value()) );
  }
}


void free_matrices() {
  
  for (unsigned i=0; i<nmats.get_value(); i++) {
    free(matricesA[i]);
    free(matricesB[i]);
    free(matricesC[i]);
  }
}

int get_matrix(void) {
  
  int id; 

  if (next_matrix >= nmats.get_value())
    return -1;

		id = next_matrix++;

  return id;
}

#ifdef MANUAL
extern "C" bool __fasan_verify(const char * ptr) __attribute__((nothrow,const));
extern "C" void __fasan_touch(const char * ptr) __attribute__((nothrow,const));
extern "C" void fasan_safe_double(const double * ptr) __attribute__((nothrow));

inline double safeLoadDouble(const double * ptr) { fasan_safe_double(ptr); return *ptr; }

#define SAFE_LOAD(REF) safeLoadDouble(&(REF))
#define SAFE_STORE(REF,V) { double * ptr = &(REF); fasan_safe_double(ptr); *ptr=V; }
#define VERIFY(REF) __fasan_verify((const char*) &(REF)) // true on poison
#define TOUCH(REF) __fasan_touch((const char*) &(REF)) // true on poison


void __attribute__((noinline))
lud(double * A, double * B, double * C, unsigned N)
{
    int i,j,k;

    for (i=0 ;i<N;++i ) {
        SAFE_STORE(B[i*N + i],1);

        for( j=(i+1);j<N;++j ) {
            SAFE_STORE(B[j*N + i], SAFE_LOAD(A[j*N +i])/SAFE_LOAD(A[i*N + i]));

            for(k=(i+1);k<N;++k)
                SAFE_STORE(A[j*N + k], SAFE_LOAD(A[j*N + k]) - SAFE_LOAD(B[j*N + i])*SAFE_LOAD(A[i*N + k]));
        }

        for (k=i;k<N;++k)
            SAFE_STORE(C[i*N + k], SAFE_LOAD(A[i*N + k]));
    }
}

#else
void __attribute__((noinline))
lud(double * A, double * B, double * C, unsigned N)
{
    int i,j,k;

    for (i=0 ;i<N;++i ) {
        B[i*N + i] = 1;

        for( j=(i+1);j<N;++j ) {
            B[j*N + i]= A[j*N +i]/A[i*N + i];

            for(k=(i+1);k<N;++k)
                A[j*N + k] = A[j*N + k] - B[j*N + i]*A[i*N + k];
        }

        for (k=i;k<N;++k)
            C[i*N + k] = A[i*N + k];
    }
}
#endif

void* worker_thread() {
    int matrix_id;
        
    while ( (matrix_id = get_matrix()) >= 0) {

		double* A = matricesA[matrix_id];
		double* B = matricesB[matrix_id];
		double* C = matricesC[matrix_id];
		unsigned N = matsz.get_value();

		memset( A, 1, N*N*sizeof(double) );

//LU decomposition of A
//B = L and C = U
        lud(A, B, C, N);
		  
	}

    return NULL;
}



int main(int argc, char *argv[]) {

    if (clarg::parse_arguments(argc, argv)) {
        cerr << "Error when parsing the arguments!" << endl;
        return 1;
    }
    
    if (nmats.get_value() < 1) {
        cerr << "Error, nm must be >= 1" << endl;
        return 1;
    }

    init_matrices();
#ifdef ENABLE_OUTPUT
    struct timespec startTime, endTime;

    readTime(&startTime);
#endif
        worker_thread();
#ifdef ENABLE_OUTPUT
    readTime(&endTime);

    const double elapsedTime = timeDiff(startTime, endTime);

    printf("SIZE %d\nTIME %f\n", matsz.get_value(), elapsedTime);
#endif
    free_matrices();


    return 0;
}
